﻿/* ------------------------------------------------------------------------- */
/*
 * C O M P A N Y   P R O P R I E T A R Y
 * use or disclosure of data contained in this file is subject to the
 * requirements an the end of this file
 *
 * CLASSIFICATION: OFFEN / UNCLASSIFIED
 */
/* ------------------------------------------------------------------------- */

/* ######################################################################### */
/** \file M44_auxSlamFunctions.cs
 * \brief	This file contains the auxiliary Slam Functions functions 
 *  
 * This file contains the auxiliary slam functions
 * 
 *
 * PROJEKT:   NETWORKED MULTI-ROBOT SYSTEMS
 *
 * Copyright (c) 2008 SENER - Universidad Politécnica de Madrid.
 *
 * \author    Gonzalo Rodríguez \n
 *            Universidad Politécnica de Madrid \n
 *            Grupo de Robótica y Cibernética \n
 * \version   0.01
 * \date      2010-02-16
 *
 * \n \n
 * Versionshistory: \n
 * -----------------
 * - Version 0.01:   Gonzalo RODRIGUEZ                       2010-02-16 \n
 *      First implementation of the M44_auxSlamFunctions. Including error_tests and AdaptLRM2LocalMap_Slam
 *      
/* ######################################################################### */
using System;
/*This is for using lists*/
using System.Collections;
using System.Collections.Generic;
using System.Text;
/*Using Interfaces classes*/
using Interfaces;
using Interfaces.Basis;
using Interfaces.Complex;
/*Using the C Sharp Matrix Library*/
using NMRS.CSML;
/*Using the 3D rendering engine modules*/
using xna = Microsoft.Xna.Framework;


/* ------------------------------------------------------------------------- */
/**	\namespace NMRS.M44_SensorFusionAndMapping
 *
 *  \brief	This is the namespace of the M44 Sensor Fusion and Mapping module
 */
/* ------------------------------------------------------------------------- */
namespace NMRS.M44_SensorFusionAndMapping
{
    /* ------------------------------------------------------------------------- */
    /**	\class M44_SlamErrors
     * 
     *  \brief  This is the class of the M44_auxSlamFunctions
     *
     * 
     */
    /* ------------------------------------------------------------------------- */
    public static class M44_auxSlamFunctions
    {
        /* ------------------------------------------------------------------------- */
        /**	\fn public static float error_tests(float[,] Map1, float[,] Map2, int s_Nm1, int s_Nm2)
         * 
         *  \brief Given to Maps this functions computes the differences between them and gives the error value
         *  
         *  \param[in] float[,] Map1
         *  \param[in] float[,] Map2
         *  \param[in] int s_Nm1
         *  \param[in] int s_Nm2,
         *  
         *  \param[out] float error
         */
        /* ------------------------------------------------------------------------- */
        public static float error_tests(float[,] Map1, float[,] Map2, int s_Nm1, int s_Nm2)
        {
            double error_temp;
            double acumulated_error;
            double total_error;
            acumulated_error = 0;
            for (int i = 0; i < s_Nm1; i++)
            {
                for (int j = 0; j < s_Nm2; j++)
                {
                    error_temp = Math.Pow((double)(Map1[i, j] - Map2[i, j]), 2);
                    acumulated_error = acumulated_error + error_temp;
                }
            }
            total_error = Math.Sqrt(acumulated_error);
            return ((float)total_error);
        }

        /* ------------------------------------------------------------------------- */
        /**	\fn public static float[,] AdaptLRM2LocalMap_Slam(int layer,
                                                cl_Map2D LowResMapGlobal,
                                                cl_Map2D HighResMap,
                                                int[] RobotCell_tested)
         * 
         *  \brief Adapt the LRM map to a local map, with some slam variatons
         *  
         *  \param[in] int layer
         *  \param[in] cl_Map2D LowResMapGlobal
         *  \param[in] cl_Map2D HighResMap
         *  \param[in] int[] RobotCell_tested
         *  
         *  \param[out] float[,] tf_LocalMap
         */
        /* ------------------------------------------------------------------------- */

        public static float[,] AdaptLRM2LocalMap_Slam(int layer,
                                                cl_Map2D LowResMapGlobal,
                                                cl_Map2D HighResMap,
                                                int[] RobotCell_tested)
        {
            //Parameter definition
            int s_Scale_global = LowResMapGlobal.us_Scale;
            double s_Scale_local = (double)HighResMap.us_Scale/10;
            int s_Nm1 = HighResMap.us_Nrows;
            int s_Nm2 = HighResMap.us_Ncols;
            int s_Nm1_global = LowResMapGlobal.us_Nrows;
            int s_Nm2_global = LowResMapGlobal.us_Ncols;
            int[] Cell_local_global = new int[2];
            int i0, i1, j0, j1;
            double d_nScale;
            float[,] tf_LocalMap;

            d_nScale = (double)s_Scale_local / s_Scale_global;

            tf_LocalMap = new float[s_Nm1, s_Nm2];

            //%First we go trough the LRM, we pick up the necesary cells of LRM to build the layers of Local Map
            //%A compulsory condition is s_Nm1*n*(1/2)=integer

            //LowResMap Area definition
            i0 = (int)(RobotCell_tested[0] - s_Nm1 * d_nScale * 0.5);
            if (i0 < 0)
                i0 = 0;
            i1 = (int)(RobotCell_tested[0] + s_Nm2 * d_nScale * 0.5 - 1);
            if (i1 >= s_Nm1_global)
                i1 = s_Nm1_global - 1;
            j0 = (int)(RobotCell_tested[1] - s_Nm2 * d_nScale * 0.5 - 1);
            if (j0 < 0)
                j0 = 0;
            j1 = (int)(RobotCell_tested[1] + s_Nm2 * d_nScale * 0.5 - 2);
            if (j1 >= s_Nm2_global)
                j1 = s_Nm2_global - 1;

            //LowResMap area extraction

            for (int k1 = i0; k1 < i1; k1++)
            {
                for (int k2 = j0; k2 < j1; k2++)
                {
                    for (int i = (int)(k1 * (1 / d_nScale) - i0 * (1 / d_nScale)); i < (int)(k1 * (1 / d_nScale) - i0 * (1 / d_nScale) + (1 / d_nScale) - 1); i++)
                    {
                        for (int j = (int)(k2 * (1 / d_nScale) - j0 * (1 / d_nScale)); j < (int)(k2 * (1 / d_nScale) - j0 * (1 / d_nScale) + (1 / d_nScale)); j++)
                        {
                            if ((k1 < 0) || (k2 < 0))
                                tf_LocalMap[i, j] = 0;
                            else
                                if (layer == 1)
                                    tf_LocalMap[i, j] = LowResMapGlobal.MapCell[k1, k2].f_Gradient_x;
                                else if (layer == 2)
                                    tf_LocalMap[i, j] = LowResMapGlobal.MapCell[k1, k2].f_Gradient_y;
                                else if (layer == 3)
                                    tf_LocalMap[i, j] = LowResMapGlobal.MapCell[k1, k2].f_Height;

                        }
                    }
                }
            }
            return tf_LocalMap;
        }
    }
}

/* ------------------------------------------------------------------------- */
/*
 * RIGHT OF USE. This document may neither be passed on to third parties or
 * reproduced nor its contents utilized or divulged without the expressed
 * prior permission of the EUROPEAN DEFENCE AGENCY, or any national government
 * having rights on it. In case of contravention, the offender shall be
 * liable for damages.
 *
 * ATTENTION! DEFENCE MATERIAL. This document may contain data which is subject
 * to export control. For the export of this data an export license is
 * required.
 *
 * COMPANY PROPRIETARY. This document contains proprietary information and
 * may only be used by the recipient for the prescribed purposes and may
 * neither be reproduced in any form nor the document itself or its content
 * divulged to third parties without our expressed prior written permission.
 *
 * COPYRIGHT (C) Diehl BGT Defence GmbH & Co. KG; 2008; All rights reserved; 
 *
 * DISTRIBUTION AND USAGE RIGHTS: Restricted to the NETWORKED MULTI-ROBOT
 * SYSTEMS Project Consortium, participation governments, prime contractor,
 * subcontractors and vendors in accordance with contract / subcontract
 * clauses and / or other specified written agreements.
 */
/* ------------------------------------------------------------------------- */